function [CFM, qMF] = multvecattdet_quest(vM, vF, stepNum) %#codegen % FIXME: 零误差测试未通过
%MULTVECATTDET_QUEST 多矢量定姿算法-QUEST方法
%
% Input Arguments:
% # vM: 3*n矩阵，M系下的一组参考矢量，n表示矢量数，单位无
% # vF: 3*n矩阵，F系下的一组参考矢量，n表示矢量数，单位无
% # stepNum: 迭代步数，默认为200
%
% Output Arguments:
% # CFM: 3*3矩阵，姿态矩阵，由F系到M系的转换矩阵，单位无
% # qMF: 4*1向量，姿态矩阵对应的四元数，单位无
%
% References:
% # 刘朝山，刘光斌，王新国，李爱华 弹载星敏感器原理及系统应用[M]. 北京: 国防工业出版社, 2010
% # 郑振宇，高延滨，苑志江，郑智林 捷联惯导系统惯性系粗对准原理及试验[J]. 中国航海 2015
% # M.D.Shuster S.D.Oh Three-Axis Attiude Determination from Vector Observation. GUIDANCE AND CONTROL 1979
% #《应用导航算法工程基础》“多矢量定姿算法及其误差”

if nargin < 3
    stepNum = 200;
end

[K, ~, Z, S, sigma] = davqquadmat(vM, vF);

% 求S的伴随矩阵
adjS = adjoint(S);
kappa = adjS(1, 1) + adjS(2, 2) + adjS(3, 3);

% 求最大特征值
Delta = det(S); % det 求行列式的值
a = sigma*sigma - kappa;
b = sigma*sigma + Z'*Z;
c = Delta + Z'*S*Z;
d = Z'*(S*S)*Z;
% lambdaMax求取，方式一适合工程应用，方式二适合算法分析仿真
if stepNum>0
    lambdaMax = 1;
    for i = 1:stepNum
        lambdaMax = lambdaMax - (lambdaMax^4 - (a+b)*lambdaMax^2 - c*lambdaMax + (a*b - c*sigma - d))/(4*lambdaMax^3 - 2*(a+b)*lambdaMax - c);
    end
else
    [~, D] = eig(K);
    lambdaMax = max(diag(D));
end

alpha = lambdaMax*lambdaMax - sigma*sigma + kappa;
beta = lambdaMax - sigma;
X = (alpha*eye(3) + beta*S + S*S)*Z;
mu = (lambdaMax + sigma)*alpha - Delta;

qBarOpt = [X; mu]/sqrt(mu*mu + X(1, 1)*X(1, 1) + X(2, 1)*X(2, 1) + X(3, 1)*X(3, 1)); % REF2 69式 求解四元数
% 规范化
q = real(qBarOpt);
for i = 1:100
    q = quatnormalize_pgs(q')';
end
% 四元数定义差异，将转角控制在[-pi, pi]
qMF = quatwrap(q([4 1 2 3])')';

% 四元数对应的姿态矩阵
CFM = quat2dcm_cg(qMF');
end